import lejos.nxt.Button;
import lejos.nxt.ButtonListener;
import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.robotics.navigation.Pilot;
import lejos.robotics.navigation.TachoPilot;


public class Test implements ButtonListener {

	Pilot pilot;
	
	private int currentTest = 0;
	private InterfaceController interfaceController = new InterfaceController();

	
	public static void main(String args[]) {
		Pilot pilot = new TachoPilot(55, 55, 108.3f, Motor.B, Motor.C, false);
		Test test = new Test();
		test.pilot = pilot;
		
		//test.forwardTest();
		test.start();
		
		
		//Button.waitForPress();
	}
	
	public void start() {
		Button.LEFT.addButtonListener(this);
		Button.RIGHT.addButtonListener(this);
		Button.ENTER.addButtonListener(this);
		LCD.drawString("Forward test", 0, 0);
		
		
		Button.ESCAPE.waitForPressAndRelease();
	}
	
	public void forwardTest(){
		pilot.travel(4000);
	}
	
	public void rotationTest(){
		int rotationAngle = interfaceController.promptForInteger("Size angle",0,10);
		pilot.rotate(rotationAngle);
	}
	
	public void deviationTest(){
		pilot.travel(5000);
	}

	public void buttonPressed(Button arg0) {
		// TODO Auto-generated method stub
		
	}

	public void buttonReleased(Button arg0) {
		if(arg0.equals(Button.ENTER)) {
			LCD.clearDisplay();
			LCD.drawString("Testing...", 0, 0);
			if(currentTest == 0) {
				forwardTest();
			} else if(currentTest == 1) {
				rotationTest();
			} else if(currentTest == 2) {
				deviationTest();
			}
		} else if(arg0.equals(Button.LEFT)) {
			currentTest = (currentTest - 1) % 3;
		} else if(arg0.equals(Button.RIGHT)) {
			currentTest = (currentTest + 1) % 3;
		}
		
		LCD.clearDisplay();
		if(currentTest == 0) {
			LCD.drawString("Forward test", 0, 0);
		} else if(currentTest == 1) {
			LCD.drawString("Rotation test", 0, 0);
		} else if(currentTest == 2) {
			LCD.drawString("Deviation test", 0, 0);
		}
	}
}
